#include "servo.h"
#include "tim.h"
void Servo_Init(void)
{
    // 启动所有舵机通道的PWM输出
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
}
void Servo_SetAngle(uint8_t ch, float Angle)  //输入需要设置的角度
{
	if (Angle > 180)     Angle = 180;
	else if (Angle < 0)  Angle = 0;
	
	// 计算PWM值：0°对应500us，180°对应2500us
	uint32_t pulse = (uint32_t)(Angle / 180.0 * 2000.0 + 500.0);
	
	switch(ch)
	{
		case 3:  
			__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulse);
			break;
		case 4:  
			__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pulse);
			break;
		case 1:  
			__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pulse);
			break;
		case 2:  
			__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, pulse);
			break;
	}
}
